/*
 * @Description: ICP2 匹配模块
 * @Author: imagoodsong
 * @Date: 2020-10-13
 */
#ifndef LIDAR_LOCALIZATION_MODELS_REGISTRATION_ICP2_REGISTRATION_HPP_
#define LIDAR_LOCALIZATION_MODELS_REGISTRATION_ICP2_REGISTRATION_HPP_

#include "lidar_localization/models/registration/registration_interface.hpp"
#include <pcl/kdtree/kdtree_flann.h>

namespace lidar_localization {
class ICP2Registration: public RegistrationInterface {
  public:
    ICP2Registration(const YAML::Node& node);
    ICP2Registration(float max_dis, float euclidean_change, float euclidean_eps, int max_iter);

    bool SetInputTarget(const CloudData::CLOUD_PTR& input_target) override;
    bool ScanMatch(const CloudData::CLOUD_PTR& input_source, 
                   const Eigen::Matrix4f& predict_pose, 
                   CloudData::CLOUD_PTR& result_cloud_ptr,
                   Eigen::Matrix4f& result_pose) override;
  
  private:
    bool SetRegistrationParam(float max_dis, float euclidean_change, float euclidean_eps, int max_iter);

    CloudData::CLOUD_PTR target_;
    pcl::KdTreeFLANN<CloudData::POINT>* kd_tree_;
    float max_dis_;
    float euclidean_change_;
    float euclidean_eps_;
    int max_iter_;
};
}

#endif